import logging
import os

# 配置日志
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')

def pwm_export(pwmid):
    try:
        with open(f'/sys/class/pwm/pwmchip{pwmid}/export', 'wb') as f:
            f.write("0".encode())
        #logging.info(f"成功导出PWM设备: pwmchip{pwmid}")
    except OSError as e:
        logging.error(f"导出PWM设备时出错: {e}")

def pwm_unexport(pwmid):
    try:
        with open(f'/sys/class/pwm/pwmchip{pwmid}/unexport', 'wb') as f:
            f.write("0".encode())
        #logging.info(f"成功取消导出PWM设备: pwmchip{pwmid}")
    except OSError as e:
        logging.error(f"取消导出PWM设备时出错: {e}")

def pwm_config(pwmid, pwmPeriod, pwmDutyCycle):
    try:
        with open(f'/sys/class/pwm/pwmchip{pwmid}/pwm0/period', 'wb') as f:
            f.write(str(pwmPeriod).encode())
        with open(f'/sys/class/pwm/pwmchip{pwmid}/pwm0/duty_cycle', 'wb') as f:
            f.write(str(pwmDutyCycle).encode())
        #logging.info(f"成功配置PWM设备: pwmchip{pwmid}")
    except OSError as e:
        logging.error(f"配置PWM设备时出错: {e}")

def pwm_enable(pwmid):
    try:
        with open(f'/sys/class/pwm/pwmchip{pwmid}/pwm0/enable', 'wb') as f:
            f.write("1".encode())
        logging.info(f"成功使能PWM设备: pwmchip{pwmid}")
    except OSError as e:
        logging.error(f"使能PWM设备时出错: {e}")

def pwm_disable(pwmid):
    try:
        with open(f'/sys/class/pwm/pwmchip{pwmid}/pwm0/enable', 'wb') as f:
            f.write("0".encode())
        logging.info(f"成功禁用PWM设备: pwmchip{pwmid}")
    except OSError as e:
        logging.error(f"禁用PWM设备时出错: {e}")

def pwm_polarity(pwmid, pwmPolarity):
    try:
        with open(f'/sys/class/pwm/pwmchip{pwmid}/pwm0/polarity', 'wb') as f:
            f.write(pwmPolarity.encode())
        #logging.info(f"成功设置PWM极性: pwmchip{pwmid}")
    except OSError as e:
        logging.error(f"设置PWM极性时出错: {e}")

def pwm_structure(pwmid, pwmPeriod, pwmDutyCycle, pwmPolarity):
    pwm_export(pwmid)
    pwm_config(pwmid, pwmPeriod, pwmDutyCycle)
    pwm_polarity(pwmid, pwmPolarity)
    pwm_enable(pwmid)

def pwm_destruction(pwmid):
    pwm_disable(pwmid)
    pwm_unexport(pwmid)

def pwm_out_all():
    for i in range(1, 3):
        pwm_disable(i)
        pwm_unexport(i)

class PWM_Servo(object):
    def __init__(self, pwmid, freq=50, min_width=500, max_width=2500, deviation=0, control_speed=False):
        self.targetPos = 0
        self.positionInc = 0.0
        self.posChanged = False
        self.Time = 0
        self.pwmid = pwmid
        self.Position = 1500
        self.freq = freq
        self.min_width = min_width
        self.max_width = max_width
        self.deviation = deviation
        self.control_speed = control_speed
        self.pwmPeriod = int(1e9 / freq)  # 计算 PWM 周期
        self.stepTime = 20
        self.incTimes = 0
        self.servoRunning = False

        pwm_structure(self.pwmid, self.pwmPeriod, 0, "normal")

        if control_speed:
            import threading
            t = threading.Thread(target=self.updatePosition)
            t.setDaemon(True)
            t.start()

    def setPosition(self, pos, time=0):
        if pos < self.min_width or pos > self.max_width:
            return
        if time == 0:
            self.Position = pos
            self.set_pwm_duty_cycle(self.Position + self.deviation)
        else:
            self.posChanged = True
            self.targetPos = pos
            self.Time = time

    def getPosition(self):
        return self.Position

    def updatePosition(self):
        import time
        while True:
            if self.posChanged:
                self.Time = self.Time
                self.Position = self.targetPos
                self.posChanged = False
                self.incTimes = int(self.Time / self.stepTime)
                self.positionInc = (self.Position - self.getPosition()) / self.incTimes
                self.servoRunning = True

            if self.servoRunning:
                self.incTimes -= 1
                if self.incTimes <= 0:
                    self.Position = self.targetPos
                    self.servoRunning = False
                else:
                    self.Position += self.positionInc
                self.set_pwm_duty_cycle(int(self.Position + self.deviation))
            time.sleep(0.02)

    def set_pwm_duty_cycle(self, pwmDutyCycle):
        pwm_config(self.pwmid, self.pwmPeriod, pwmDutyCycle * 1000)

# 示例调用
if __name__ == "__main__":
    pwm_out_all()  # 清理所有可能已经导出的PWM设备
